#pragma config(UART_Usage, UART2, uartNotUsed, baudRate4800, IOPins, None, None)
#pragma config(Sensor, dgtl1,  leftRight,      sensorTouch)
#pragma config(Sensor, dgtl2,  strat,          sensorTouch)
#pragma config(Sensor, dgtl7,  Indicator,      sensorDigitalOut)
#pragma config(Motor,  port2,           flipper,       tmotorVex393, openLoop)
#pragma config(Motor,  port3,           backLeft,      tmotorVex269, openLoop)
#pragma config(Motor,  port4,           conveyer,      tmotorVex393, openLoop)
#pragma config(Motor,  port5,           backRight,     tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port6,           raiseFL,       tmotorVex393, openLoop)
#pragma config(Motor,  port7,           raiseBL,       tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port8,           raiseBR,       tmotorVex393, openLoop)
#pragma config(Motor,  port9,           raiseFR,       tmotorVex393, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define abs(X) ((X < 0) ? -1 * X : X)
#pragma platform(VEX)
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)
#include "Vex_Competition_Includes.c"

void tankDriveUser();
void autonSpin(float Deg);
void conveyerControlUser();
task userRaiserControl();
void autonDrive(int power, float distance);
void spinnerUserControl();

float SPEED=0;
float DIS = 12.375;
float ROTAT_SPEED=5;
void pre_auton()
{
        nMotorEncoder[raiseBL]=0;
        nMotorEncoder[raiseFL]=0;
        ClearTimer(T1);
}

task autonomous()
{
        if(SensorValue(strat)==0)	{
        autonDrive(127,18);
                if(SensorValue(leftRight)==1)	{
                        autonSpin(-90);
										} else {
                        autonSpin(90);
								}
                autonDrive(127,9);
                if(SensorValue(leftRight)==1) {
                        autonSpin(90);
										} else {
                        autonSpin(-90);
								}
                int power =100;
                motor[conveyer]=power;
                autonDrive(80,24);
                power =0;
                motor[conveyer]=power;

							} else {
			}
}

task usercontrol()
{
        StartTask(userRaiserControl);
        while (true)
				{
                tankDriveUser();
                conveyerControlUser();
                spinnerUserControl();
				}
}
void tankDriveUser() {
        int left=0, right=0;
        int threshold = 5;
        if(abs(vexRT[Ch3])>=threshold)
                left = vexRT[Ch3];
        if(abs(vexRT[Ch2])>=threshold)
                right = vexRT[Ch2];
        motor[backLeft]=left;
        motor[backRight]=right;
}
void conveyerControlUser()	{
        int power=0;
        if(vexRT[Btn6U]==1)	{
                power=127;
								} else if(vexRT[Btn6D]==1)	{
                power=-127;
								} else {
                power=0;
			}
        motor[conveyer]=power;
}
task userRaiserControl()	{
	int power=0, frontPower=0;
    bool value2 = true;
    while(1==1)	{
      if(vexRT[Btn5U]==1)	{
        power=60;
        value2 = false;
			} else if(vexRT[Btn5D]==1) {
        power = -100;
        frontPower = -5;
        value2 = false;
			} else	{
        power= 0;
        frontPower=5;
			}
			if(vexRT[Btn7U]==1)	{
				motor[raiseBL]=0;
        motor[raiseFL]=0;
        motor[raiseBR]=0;
        motor[raiseFR]=0; 
        value2 = true;
        SensorValue(Indicator)=0;
   		} 
			if(!value2)	{
				  motor[raiseBL]=power;
          motor[raiseFL]=power+17;
          motor[raiseBR]=power+10;
          motor[raiseFR]=power+20;
          SensorValue(Indicator)=1;
			}
	}
}
void spinnerUserControl()	{
        int power;
        if(vexRT[Btn6U]==1)	{
                power=127;
								} else if(vexRT[Btn6D]==1)	{
                power=-127;
								} else 	{
                power=0;
				}
        motor[flipper]=power;
}
void autonDrive(int power, float distance)	{
        int waitTime= (int)(SPEED*abs(power)*distance);
        motor[backLeft]=power;
        motor[backRight]=power;
        wait1Msec(waitTime);
        motor[backLeft]=0;
        motor[backRight]=0;
}

void autonSpin(float Deg)	{
        int waitTime =(int)( ROTAT_SPEED*abs(Deg));
        int left=0;
        int right=0;
        if(Deg <0)	{
                left=-127;
                right=127;
								} else {
                left=127;
                right=-127;
				}
        motor[backLeft]=left;
        motor[backRight]=right;
        wait1Msec(waitTime);
        allMotorsOff();
}